package de.koller.oceanFramework.math;

/**
 * 
 * @author Fabian Koller
 */
public class IntersectionUtil {

	/**
	 * 
	 * @param a
	 * @param b
	 * @return
	 */
	public static double dot( Vec3 a, Vec3 b ) {
		return a.getX()*b.getX() + a.getY()*b.getY() + a.getZ()*b.getZ();
	}
	
	/**
	 * 
	 * @param va
	 * @param vb
	 * @return
	 */
	public static Vec3 cross( Vec3 va, Vec3 vb ) {
		double a = va.getX();
		double b = va.getY();
		double c = va.getZ();
		double p = vb.getX();
		double q = vb.getY();
		double r = vb.getZ();
		return new Vec3( b*r-c*q, c*p-a*r, a*q-b*p );
	}
	
	/**
	 * 
	 * @param la
	 * @param lb
	 * @param planeO
	 * @param planeN
	 * @return
	 */
	public static double intersection( Vec3 la, Vec3 lb, Vec3 planeO, Vec3 planeN ) {
//		double d = dot( planeO, planeN );
		
		double numer = dot( planeN, Vec3.difVec( planeO, la ) ); //+ d;
		double denom = dot( planeN, Vec3.difVec( lb, la ) );
		
		return numer / denom;
	}
	
	/**
	 * 
	 * @param lineO
	 * @param lineN
	 * @param t
	 * @return
	 */
	public static Vec3 linePoint( Vec3 lineO, Vec3 lineN, double t ) {
		Vec3 cp = new Vec3();
		cp.setX( lineO.getX() + lineN.getX() * t );
		cp.setY( lineO.getY() + lineN.getY() * t );
		cp.setZ( lineO.getZ() + lineN.getZ() * t );
		return cp;
	}
	
	/**
	 * 
	 * @param p
	 * @param a
	 * @param b
	 * @param c
	 * @return
	 */
	public static boolean isInPolygon( Vec3 p, Vec3 a, Vec3 b, Vec3 c ) {
		// Compute vectors        
		Vec3 v0 = Vec3.difVec( c, a );
		Vec3 v1 = Vec3.difVec( b, a );
		Vec3 v2 = Vec3.difVec( p, a );

		// Compute dot products
		double dot00 = dot(v0, v0);
		double dot01 = dot(v0, v1);
		double dot02 = dot(v0, v2);
		double dot11 = dot(v1, v1);
		double dot12 = dot(v1, v2);

		// Compute barycentric coordinates
		double invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
		double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
		double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

		// Check if point is in triangle
		return (u > 0) && (v > 0) && (u + v < 1);
	}

	/**Gibt den Punkt zurück an welcher Stelle das Polygon mit dem Ray schneidet
	 * @param ra Aufpunkt des Rays
	 * @param rb Endpunkt des Rays
	 * @param a Vertex a des Polygon
	 * @param b Vertex b des Polygon
	 * @param c Vertex c des Polygon 
	 * @return Der Punkt der Überschneidung, null wenn keine. */
	public static Vec3 getRayTrace( Vec3 ra, Vec3 rb, Vec3 a, Vec3 b, Vec3 c ) {
		Vec3 pn = cross( Vec3.difVec( b, a ), Vec3.difVec( c, a ) ).normalize();
		Vec3 ln = Vec3.difVec( rb, ra );
		
		double iv = intersection( ra, rb, a, pn );
//		System.out.println( iv );
		if( iv < 0 || iv > 1 )
			return null;
		
		Vec3 ip = linePoint( ra, ln, iv );
		if( isInPolygon( ip, a, b, c ) )
			return ip;
		else
			return null;
	}
	
	/**
	 * 
	 * @param args
	 */
	public static void main( String... args ) {
		System.out.println(
				getRayTrace(	new Vec3( 0, 0, 0 ), 
								new Vec3( 1.5, -2, 0 ),
								new Vec3( 1, -1, -1 ),
								new Vec3( 1, 1, 0 ),
								new Vec3( 1, -1, 1 ) ) );
								
	}
}
